#include <gluon_state_publisher/gluon_state_publisher.h>
#include <ros/ros.h>
#include <std_msgs/Float64.h>
#include <geometry_msgs/Pose.h>
#include <rocr6_msgs/Move.h>
#include <std_msgs/UInt16.h>
#include <math.h>
#include <gluon_state_publisher/ik6s.h>
#include <gluon_state_publisher/priority_path.h>

double joint_pos[JOINT_MAX] = {0,0,0,0,0,0};

using namespace std;
//定义的宏
#define PI 3.1415926535897931
#define DATA_GAIN  10
#define EPSINON 1e-4  // 精度，小于它就认为是0
#define ALLOW_MAX_DIFF (1 / 180 * PI)  /* 距离上一轴角度的最大允许偏差 */
#define JOINT_MAX   6
#define JOINT_SIZE 6

double last[JOINT_MAX] = {0, 0, 0, 0, 0, 0};
double xlast[JOINT_MAX] = {0, 0, 0, 0, 0, 0};
double ax[6];
double *dh_ptr;
double itheta[48];
double sp[16];
static int gn = 1;      //逆解组号
static int dir = 0;

class gluon_state_publisher
{
  //私有权限
private:
  /* data */
  ros::NodeHandle n;
  double joint[JOINT_SIZE] = {0, 0, 0, 0, 0, 0};
  void callback(const rocr6_msgs::Move::ConstPtr &msg);
  void j1_CB(const std_msgs::Float64::ConstPtr & msg);
  void j2_CB(const std_msgs::Float64::ConstPtr & msg);
  void j3_CB(const std_msgs::Float64::ConstPtr & msg);
  void j4_CB(const std_msgs::Float64::ConstPtr & msg);
  void j5_CB(const std_msgs::Float64::ConstPtr & msg);
  void j6_CB(const std_msgs::Float64::ConstPtr & msg);
  rocr6_msgs::Move move;

  //void innfos_vision::callback(const rocr6_msgs::Move::ConstPtr &msg)

//公共权限
public:
  gluon_state_publisher(/* args */);
};
//行为
gluon_state_publisher::gluon_state_publisher(/* args */)
{
  ros::NodeHandle nh;
  ros::Rate loop_rate(50);

  //订阅角度话题
  ros::Subscriber sub_joint_1 = nh.subscribe("gluon/joint_1_position", 1, &gluon_state_publisher::j1_CB,this);
  ros::Subscriber sub_joint_2 = nh.subscribe("gluon/joint_2_position", 1, &gluon_state_publisher::j2_CB,this);
  ros::Subscriber sub_joint_3 = nh.subscribe("gluon/joint_3_position", 1, &gluon_state_publisher::j3_CB,this);
  ros::Subscriber sub_joint_4 = nh.subscribe("gluon/joint_4_position", 1, &gluon_state_publisher::j4_CB,this);
  ros::Subscriber sub_joint_5 = nh.subscribe("gluon/joint_5_position", 1, &gluon_state_publisher::j5_CB,this);
  ros::Subscriber sub_joint_6 = nh.subscribe("gluon/joint_6_position", 1, &gluon_state_publisher::j6_CB,this);
  ros::Subscriber sub_vi = nh.subscribe<rocr6_msgs::Move>("/target_sub",100,&gluon_state_publisher::callback,this);

  //发布机械臂状态
  ros::Publisher pub_states = nh.advertise<sensor_msgs::JointState>("joint_states", 100);

  sensor_msgs::JointState joint_state;

  nh.getParam("/gluon_state_publisher/joint_names", joint_state.name);
  nh.getParam("/gluon_state_publisher/joint_position", joint_state.position);

  // for(int i=0; i< JOINT_MAX; i++)
  // {
  //    printf("%s\r\n", joint_state.name[i].c_str());
  // }

  while(ros::ok())
  {
    
    joint_state.header.stamp = ros::Time::now();
    for(int i=0; i<JOINT_MAX; i++)
    {
      joint_state.position[i] = joint_pos[i];  
    }
    pub_states.publish(joint_state);

    ros::spinOnce();
    loop_rate.sleep();
  }

}
/**
  * @brief  关节一接收回调函数
  * @param  消息结构体指针
  * @retval 无
  */
void gluon_state_publisher::j1_CB(const std_msgs::Float64::ConstPtr & msg)
{
  joint_pos[0] = msg->data;
  joint_pos[0] = joint[0];
}

/**
  * @brief  关节二接收回调函数
  * @param  消息结构体指针
  * @retval 无
  */
void gluon_state_publisher::j2_CB(const std_msgs::Float64::ConstPtr & msg)
{
  joint[1] = msg->data;
  joint_pos[1] = joint[1];
}
/**
  * @brief  关节三接收回调函数
  * @param  消息结构体指针
  * @retval 无
  */
void gluon_state_publisher::j3_CB(const std_msgs::Float64::ConstPtr & msg)
{
  joint[2] = msg->data;
  joint_pos[2] = joint[2];
}
/**
  * @brief  关节四接收回调函数
  * @param  消息结构体指针
  * @retval 无
  */
void gluon_state_publisher::j4_CB(const std_msgs::Float64::ConstPtr & msg)
{
  joint[3] = msg->data;
  joint_pos[3] = joint[3];
}
/**
  * @brief  关节五接收回调函数
  * @param  消息结构体指针
  * @retval 无
  */
void gluon_state_publisher::j5_CB(const std_msgs::Float64::ConstPtr & msg)
{
  joint[4] = msg->data;
  joint_pos[4] = joint[4];
}
/**
  * @brief  关节六接收回调函数
  * @param  消息结构体指针
  * @retval 无
  */
void gluon_state_publisher::j6_CB(const std_msgs::Float64::ConstPtr & msg)
{
  joint[5] = msg->data;
  joint_pos[5] = joint[5];
}
/* 角度值转弧度值 */
inline double toRadian(double degree)
{
    return degree * PI / 180;
}
/* 逆解函数
* @Joint 6轴各关节值结构体，单位弧度
* @lastJoints 上一次计算的关节值结构体
* @pose 要解逆解的位姿矩阵
*/
void gluon_state_publisher::callback(const rocr6_msgs::Move::ConstPtr &msg)
{
    double val,sel;
    int rst = 0;
    ax[0] = msg->a1;
    ax[1] = msg->a2;
    ax[2] = msg->a3;
    ax[3] = msg->a4;
    ax[4] = msg->a5;
    ax[5] = msg->a6;

    rst = ik6s(ax, dh_ptr, itheta);

    if(rst)
    {
      return;
    }
    else
    {
      priority_path(dir,itheta,xlast, &val, &sel);
      if((val==0)&&(sel==0))
      {
        printf("没有满足条件的最优解. \r\n");
        return;
      }
      if(sel==0)
      {
        gn = 0;
      }
      else
      {
        gn = sel-1;
      }
      printf("运动方向：[%d] 最小路径权重：[%f]->选择第[%d]组解\r\n",dir, val,(int)sel);
      xlast[0] = itheta[gn+0];
      xlast[1] = itheta[gn+8];
      xlast[2] = itheta[gn+16];
      xlast[3] = itheta[gn+24];
      xlast[4] = itheta[gn+32];
      xlast[5] = itheta[gn+40];

/*
      printf("%f %f %f %f %f %f\r\n", 
            xlast[0]*180/RT_PI, 
            xlast[1]*180/RT_PI, 
            xlast[2]*180/RT_PI, 
            xlast[3]*180/RT_PI,
            xlast[4]*180/RT_PI, 
            xlast[5]*180/RT_PI
      );
*/

      joint[0] = last[0] = toRadian(xlast[0]);
      joint[1] = last[1] = toRadian(xlast[1]);
      joint[2] = last[2] = toRadian(xlast[2]);
      joint[3] = last[3] = toRadian(xlast[3]);
      joint[4] = last[4] = toRadian(xlast[4]);
      joint[5] = last[5] = toRadian(xlast[5]);

    //   printf("1关节角度为:"[%f],last[0]);
    //   printf("2关节角度为:"[%f],last[1]);
    //   printf("3关节角度为:"[%f],last[2]);
    //   printf("4关节角度为:"[%f],last[3]);
    //   printf("5关节角度为:"[%f],last[4]);
    //   printf("6关节角度为:"[%f],last[5]);


    //   sub_joint_1.publish(last[0]);
    //   pub2.publish(last[1]);
    //   pub3.publish(last[2]);
    //   pub4.publish(last[3]);  
    //   pub5.publish(last[4]);
    //   pub6.publish(last[5]);      

    }
  //}
//   else//机械臂角度控制
//   {
//     last[0] = rocr6_tf_tdata(msg->a1);
//     last[1] = rocr6_tf_tdata(msg->a2);
//     last[2] = rocr6_tf_tdata(msg->a3);
//     last[3] = rocr6_tf_tdata(msg->a4);
//     last[4] = rocr6_tf_tdata(msg->a5);
//     last[5] = rocr6_tf_tdata(msg->a6);
//   }

//   if(
//     (tx_data[0]==last[0])&&
//     (tx_data[1]==last[1])&&
//     (tx_data[2]==last[2])&&
//     (tx_data[3]==last[3])&&
//     (tx_data[4]==last[4])&&
//     (tx_data[5]==last[5])
//   )
//   {
//     //发送的角度相同，不用处理
//   }
//   else
//   {
//     tx_data[0] = last[0];
//     tx_data[1] = last[1];
//     tx_data[2] = last[2];
//     tx_data[3] = last[3];
//     tx_data[4] = last[4];
//     tx_data[5] = last[5];

//     //控制器插补模式运行
//     printf("关节角度:[% 0.4f % 0.4f % 0.4f % 0.4f % 0.4f %0.4f]\r\n", 
//         tx_data[0]/10, tx_data[1]/10, tx_data[2]/10, tx_data[3]/10, tx_data[4]/10, tx_data[5]/10);
//     rocr6_txSig(1, 0, tx_data);  
//   }
}

/**
  * @brief  主函数
  * @param  消息结构体指针
  * @retval 无
  */
int main(int argc, char** argv)
{
  ros::init(argc, argv, "gluon_state_publisher");
  gluon_state_publisher gluon_state_publisherlog;
  return 0;
}